
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_compass_qmc5883l.c
  * @author     baiyang
  * @date       2022-6-15
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_compass_qmc5883l.h"

#include <common/console/console.h>
#include <common/utility/sparse-endian.h>
/*-----------------------------------macro------------------------------------*/
#define QMC5883L_REG_CONF1 0x09
#define QMC5883L_REG_CONF2 0x0A

// data output rates for 5883L
#define QMC5883L_ODR_10HZ (0x00 << 2)
#define QMC5883L_ODR_50HZ  (0x01 << 2)
#define QMC5883L_ODR_100HZ (0x02 << 2)
#define QMC5883L_ODR_200HZ (0x03 << 2)

// Sensor operation modes
#define QMC5883L_MODE_STANDBY 0x00
#define QMC5883L_MODE_CONTINUOUS 0x01

#define QMC5883L_RNG_2G (0x00 << 4)
#define QMC5883L_RNG_8G (0x01 << 4)

#define QMC5883L_OSR_512 (0x00 << 6)
#define QMC5883L_OSR_256 (0x01 << 6)
#define QMC5883L_OSR_128 (0x10 << 6)
#define QMC5883L_OSR_64  (0x11 << 6)

#define QMC5883L_RST 0x80

#define QMC5883L_REG_DATA_OUTPUT_X 0x00
#define QMC5883L_REG_STATUS 0x06

#define QMC5883L_REG_ID 0x0D
#define QMC5883_ID_VAL 0xFF

#define QMC7983_ID_VAL 0x32
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool init(sensor_compass_qmc5883l_t qmc5883l);
static bool _check_whoami(sensor_compass_qmc5883l_t qmc5883l);
static void timer(void *parameter);
static void read(sensor_compass_backend *compass_backend);
static void _dump_registers(sensor_compass_qmc5883l_t qmc5883l);
/*----------------------------------variable----------------------------------*/
struct sensor_compass_backend_ops qmc5883l_ops;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static void sensor_compass_qmc5883l_ctor(sensor_compass_qmc5883l_t qmc5883l, gp_device_t dev, bool force_external, enum RotationEnum rotation)
{
    // 清空sensor_imu_backend结构体变量，因为sensor_imu_backend结构体有可能是申请的动态内存
    // 防止sensor_imu_backend中的变量初始为非零值。
    rt_memset(qmc5883l, 0, sizeof(struct sensor_compass_qmc5883l));

    qmc5883l->name = "QMC5883L";

    sensor_compass_backend_ctor(&qmc5883l->backend, "qmc5883");

    qmc5883l_ops = compass_backend_ops;
    qmc5883l_ops.read = read;

    qmc5883l->backend.ops = &qmc5883l_ops;
    qmc5883l->_dev = dev;

    qmc5883l->_force_external = force_external;
    qmc5883l->_rotation       = rotation;
}

sensor_compass_backend *sensor_compass_qmc5883l_probe(gp_device_t dev, bool force_external, enum RotationEnum rotation)
{
    if (!dev) {
        return NULL;
    }

    sensor_compass_qmc5883l_t sensor = (sensor_compass_qmc5883l_t)rt_malloc(sizeof(struct sensor_compass_qmc5883l));

    if (!sensor) {
        return NULL;
    }

    sensor_compass_qmc5883l_ctor(sensor, dev, force_external, rotation);

    if (!init(sensor)) {
        sensor_compass_backend_destructor(&sensor->backend);
        rt_free(sensor);
        return NULL;
    }

    return (sensor_compass_backend *)sensor;
}

static bool init(sensor_compass_qmc5883l_t qmc5883l)
{
    devmgr_take_bus(qmc5883l->_dev);

    devmgr_set_retries(qmc5883l->_dev, 10);

#if 0
    _dump_registers(qmc5883l);
#endif

    if(!_check_whoami(qmc5883l)){
         goto fail;
    }

    if (!devmgr_write_register(qmc5883l->_dev, 0x0B, 0x01, false)||
         !devmgr_write_register(qmc5883l->_dev, 0x20, 0x40, false)||
         !devmgr_write_register(qmc5883l->_dev, 0x21, 0x01, false)||
         !devmgr_write_register(qmc5883l->_dev, QMC5883L_REG_CONF1,
                        QMC5883L_MODE_CONTINUOUS|
                        QMC5883L_ODR_100HZ|
                        QMC5883L_OSR_512|
                        QMC5883L_RNG_8G, false)) {
                  goto fail;
     }

    // lower retries for run
    devmgr_set_retries(qmc5883l->_dev, 3);

    devmgr_release_bus(qmc5883l->_dev);

    //register compass instance
    devmgr_set_device_type(qmc5883l->_dev, COMPASS_DEVTYPE_QMC5883L);
    if (!sensor_compass_register_compass(devmgr_get_bus_id(qmc5883l->_dev), &qmc5883l->_instance)) {
        return false;
    }
    sensor_compass_backend_set_dev_id(qmc5883l->_instance, devmgr_get_bus_id(qmc5883l->_dev));

    console_printf("%s found on bus %u id %u address 0x%02x\n", qmc5883l->name,
           qmc5883l->_dev->d.devid_s.bus, (unsigned)devmgr_get_bus_id(qmc5883l->_dev), qmc5883l->_dev->d.devid_s.address);

    sensor_compass_backend_set_rotation(qmc5883l->_instance, qmc5883l->_rotation);

    if (qmc5883l->_force_external) {
        sensor_compass_backend_set_external(qmc5883l->_instance, true);
    }

    //Enable 100HZ
    devmgr_register_periodic_callback(qmc5883l->_dev, 10000, timer, qmc5883l);

    return true;

 fail:
    devmgr_release_bus(qmc5883l->_dev);
    return false;
}

static bool _check_whoami(sensor_compass_qmc5883l_t qmc5883l)
{
    uint8_t whoami;
    //Affected by other devices,must read registers 0x00 once or reset,after can read the ID registers reliably
    devmgr_read_registers(qmc5883l->_dev, 0x00,&whoami,1);
    if (!devmgr_read_registers(qmc5883l->_dev, 0x0C, &whoami,1) ||
            whoami != 0x01){
        return false;
    }
    if (!devmgr_read_registers(qmc5883l->_dev, QMC5883L_REG_ID, &whoami,1) ||
            (whoami != QMC5883_ID_VAL && whoami != QMC7983_ID_VAL)){
        return false;
    }

    if (whoami == QMC7983_ID_VAL) {
        qmc5883l->name = "QMC7983";
    }

    return true;
}

static void timer(void *parameter)
{
    sensor_compass_qmc5883l_t qmc5883l = (sensor_compass_qmc5883l_t)parameter;

#pragma pack(1)
    struct {
        le16_t rx;
        le16_t ry;
        le16_t rz;
    } buffer;
#pragma pack()

    const float range_scale = 1000.0f / 3000.0f;

    uint8_t status;
    if(!devmgr_read_registers(qmc5883l->_dev, QMC5883L_REG_STATUS,&status,1)){
        return;
    }
    //new data is ready
    if (!(status & 0x04)) {
        return;
    }

    if(!devmgr_read_registers(qmc5883l->_dev, QMC5883L_REG_DATA_OUTPUT_X, (uint8_t *) &buffer, sizeof(buffer))){
        return ;
    }

    int16_t x = (int16_t)(le16toh(buffer.rx));
    int16_t y = (int16_t)(le16toh(buffer.ry));
    int16_t z = (int16_t)(le16toh(buffer.rz));

#if 0
    console_printf("mag.x:%d\n",x);
    console_printf("mag.y:%d\n",y);
    console_printf("mag.z:%d\n",z);
#endif

    Vector3f_t field = {x * range_scale , y * range_scale, z * range_scale };

    // rotate to the desired orientation
    if (sensor_compass_backend_is_external(qmc5883l->_instance)) {
        vec3_rotate(&field, ROTATION_YAW_90);
    }

    sensor_compass_backend_accumulate_sample(&qmc5883l->backend, &field, qmc5883l->_instance, 20);
}

static void read(sensor_compass_backend *compass_backend)
{
    sensor_compass_backend_drain_accumulated_samples(compass_backend, ((sensor_compass_qmc5883l_t)compass_backend)->_instance, NULL);
}

static void _dump_registers(sensor_compass_qmc5883l_t qmc5883l)
{
    console_printf("QMC5883L registers dump\n");
    for (uint8_t reg = QMC5883L_REG_DATA_OUTPUT_X; reg <= 0x30; reg++) {
        uint8_t v;
        devmgr_read_registers(qmc5883l->_dev, reg,&v,1);
        console_printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
        if ((reg - ( QMC5883L_REG_DATA_OUTPUT_X-1)) % 16 == 0) {
            console_printf("\n");
        }
    }
}

/*------------------------------------test------------------------------------*/


